#include "stdafx.h"
#include <cassert>
#include "cvxAnn.h"


CvxAnn::CvxAnn(const int dim, const int k)
{
	m_nPts = 0;					
	m_dataPts = NULL;				
	m_queryPt = NULL;				
	m_nnIdx = NULL;					
	m_dists = NULL;					
	m_kdTree = NULL;
	m_dim = dim;
	m_k   = k;

	m_queryPt = annAllocPt(m_dim);					
	m_dataPts = annAllocPts(40000, m_dim);		//most 40 000 point in the k-d tree	
	m_nnIdx = new ANNidx[m_k];						
	m_dists = new ANNdist[m_k];
	assert(m_queryPt);
	assert(m_dataPts);
	assert(m_nnIdx);
	assert(m_dists);

}
CvxAnn::~CvxAnn()
{
	if(m_nnIdx)
	{
		delete [] m_nnIdx;
	}
	if(m_dists)
	{
		delete [] m_dists;
	}
	if(m_kdTree)
	{
		delete m_kdTree;
	}
	annClose();
}

void CvxAnn::setTree(const vector<pair<double, double>>& in_pairs)
{
	assert(m_dim == 2);
	m_nPts = in_pairs.size();
	assert(m_nPts >0 && m_nPts <= 40000);

	TConstIPair iPair = in_pairs.begin();
	for(int i =0; iPair != in_pairs.end(); ++iPair, ++i)
	{
		m_dataPts[i][0] = (*iPair).first;
		m_dataPts[i][1] = (*iPair).second;
	}
	if (m_kdTree)
	{
		delete m_kdTree, m_kdTree = NULL;
	}

	m_kdTree = new ANNkd_tree(					// build search structure
					m_dataPts,					// the data points
					m_nPts,						// number of points
					m_dim);						// dimension of space

	assert(m_kdTree);
}

void CvxAnn::setTree(const double *pData, int size)
{
	assert(pData);
	assert(size >0 && size <= 40000);

	m_nPts = size;
	for(int i =0; i<m_nPts; ++i)
	{
		for (int j = 0; j<m_dim; ++j)
		{
			m_dataPts[i][j] = pData[i * m_dim + j];
		}
	}

	if (m_kdTree)
	{
		delete m_kdTree, m_kdTree = NULL;
	}

	m_kdTree = new ANNkd_tree(					// build search structure
								m_dataPts,		// the data points
								m_nPts,			// number of points
								m_dim);			// dimension of space

	assert(m_kdTree);
}

void CvxAnn::query(const double x, const double y, vector<double> &dis, vector<int> &idx)
{
	assert(dis.size() == idx.size());
	assert(dis.size() == m_k);

	m_queryPt[0] = x;
	m_queryPt[1] = y;

	m_kdTree->annkSearch(						// search
				m_queryPt,						      // query point
				m_k,						         // number of near neighbors
				m_nnIdx,							// nearest neighbors (returned)
				m_dists,							// distance (returned)
				0);
	for (int i = 0; i<m_k; ++i)
	{
		dis[i] = sqrt(m_dists[i]);
		idx[i] = m_nnIdx[i];
	}	
}

void CvxAnn::query(const double *pData, double *pDis, int *pIdx)
{
	assert(pData);
	assert(pDis);
	assert(pIdx);

	for (int i = 0; i<m_dim; ++i)
	{
		m_queryPt[i] = pData[i];
	}

	m_kdTree->annkSearch(						// search
						m_queryPt,				// query point
						m_k,					// number of near neighbors
						m_nnIdx,				// nearest neighbors (returned)
						m_dists,				// distance (returned)
						0);
	for (int i = 0; i<m_k; ++i)
	{
		pDis[i] = sqrt(m_dists[i]);
		pIdx[i] = m_nnIdx[i];
	}	
}